#include "pch.hpp"
#include "TPhysicsActor.hpp"
//#include "CreateMesh.h"




TPhysicsActor::TPhysicsActor(const ActorDesc& desc, const notify_t& fn)
	: m_destroyed(fn)
	, m_boundingSphere(desc.boundingSphere())
	, m_spheres(desc.spheres)
	, m_boxes(desc.boxes)

	, m_state(desc.kinematics())
{
	m_state.invWorldInertiaTensor = m_state.rotation * m_state.invInertiaTensor * m_state.rotation.transpose();
	m_state.angularVelocity = math::transform(m_state.invWorldInertiaTensor, m_state.angularMomentum);
}
///////////////////////////////////////////////////////////////////////////////////////////////////

TPhysicsActor::~TPhysicsActor()
{
	m_destroyed(this);
}
///////////////////////////////////////////////////////////////////////////////////////////////////




void TPhysicsActor::update(float_t dt)
{
	//
	// Some words about the next calculations: Most of that stuff is done in world space.
	//
	// The Reason:
	// This is because the collision detection gives us a point/line/plane in world space as well.
	// If those calculations would be done in object space, all vectors from the collision detection
	// would be needed to be transfered into object space, before calculating the response.
	//
	// It also makes it easier for the user to apply arbitrary forces to objects, like gravity.
	// It would be a chore to first transform the gravity vector(0,-9.81,0) into object space,
	// and then applying that force on the actor.
	// The downside is, that any force, needed to be applied to the actor's body directly needs to be
	// transformed into world space.
	//


	//
	// Step 1
	// Sum all forces, while splitting them up into linear and angular forces
	//

	vector3 linearForce;
	vector3 torque;

	foreach(const Force& f, m_forces)
	{
		linearForce += f.second;

		// The amount of force, being applied to angular movement Fa equals:
		// Fa = F x r
		torque += math::cross(f.first,f.second);
	}
	m_forces.clear();


	//
	// Step 2:
	// Integrate both the linear force and torque to calculate the new
	// velocity, angular velocity as well as position and rotation of the body.
	//
	// The following integrations are solved numerically, not analytically (which may be impossible).
	//

	// Integrate the velocity to get the change of the position
	m_state.position += m_state.linearVelocity * dt;
	// Integrate the linear force to get the change of the momentum (and thus the change of the velocity)
	m_state.linearVelocity += linearForce / m_state.mass * dt;

	// Now integrate the angular velocity to get the change in rotation
	m_state.rotation += math::cross(m_state.angularVelocity, m_state.rotation) * dt;
	// Integrate the torque to get the change of the angular momentum
	m_state.angularMomentum += linearForce * dt;

	// The 3 axes might not be orthogonal/normalized any more
	m_state.rotation.orthonormalize();

	m_state.invWorldInertiaTensor = m_state.rotation * m_state.invInertiaTensor * m_state.rotation.transpose();
	m_state.angularVelocity = math::transform(m_state.invWorldInertiaTensor, m_state.angularMomentum);
}
///////////////////////////////////////////////////////////////////////////////////////////////////




const ActorKinematics& TPhysicsActor::state() const
{
	return m_state;
}
///////////////////////////////////////////////////////////////////////////////////////////////////

float_t TPhysicsActor::mass() const
{
	return m_state.mass;
}
///////////////////////////////////////////////////////////////////////////////////////////////////




vector3 TPhysicsActor::position() const
{
	return m_state.position;
}
///////////////////////////////////////////////////////////////////////////////////////////////////

void TPhysicsActor::setPosition(const vector3& pos)
{
	m_state.position = pos;
}
///////////////////////////////////////////////////////////////////////////////////////////////////




matrix3x3 TPhysicsActor::rotation() const
{
	return m_state.rotation;
}
///////////////////////////////////////////////////////////////////////////////////////////////////

void TPhysicsActor::setRotation(const matrix3x3& rotation)
{
	m_state.rotation = rotation;
}
///////////////////////////////////////////////////////////////////////////////////////////////////




matrix4x4 TPhysicsActor::transform() const
{
	return matrix4x4(m_state.rotation, m_state.position);
}
///////////////////////////////////////////////////////////////////////////////////////////////////

void TPhysicsActor::setTransform(const matrix4x4& transform)
{
	m_state.position = vector3(transform._41, transform._42, transform._43);
	m_state.rotation = transform.rotation();
}
///////////////////////////////////////////////////////////////////////////////////////////////////




vector3 TPhysicsActor::velocity() const
{
	return m_state.linearVelocity;
}
///////////////////////////////////////////////////////////////////////////////////////////////////

void TPhysicsActor::setVelocity(const vector3& v)
{
	m_state.linearVelocity = v;
}
///////////////////////////////////////////////////////////////////////////////////////////////////

void TPhysicsActor::addVelocity(const vector3& v)
{
	m_state.linearVelocity += v;
}
///////////////////////////////////////////////////////////////////////////////////////////////////




vector3 TPhysicsActor::impulse() const
{
	return m_state.linearVelocity * m_state.mass;
}
///////////////////////////////////////////////////////////////////////////////////////////////////

void TPhysicsActor::setImpulse(const vector3& p)
{
	m_state.linearVelocity = p / m_state.mass;
}
///////////////////////////////////////////////////////////////////////////////////////////////////

void TPhysicsActor::addImpulse(const vector3& p)
{
	m_state.linearVelocity += p / m_state.mass;
}
///////////////////////////////////////////////////////////////////////////////////////////////////




void TPhysicsActor::addForce(const vector3& f)
{
	// Since both the position and force are assumed to be in world space,
	// We need to choose the center of mass (the position), instead of (0,0,0), which
	// would only be correct if we're doing it in object space.
	m_forces.push_back(Force(m_state.position, f));
}
///////////////////////////////////////////////////////////////////////////////////////////////////

void TPhysicsActor::addForce(const vector3& p, const vector3& f)
{
	m_forces.push_back(Force(p, f));
}
///////////////////////////////////////////////////////////////////////////////////////////////////




PhysicsActor::RayCastResult TPhysicsActor::rayCast(const ray& ray_) const
{
	RayCastResult result;

	// When the bounding sphere has not been hit,
	// then there cannot be a hit with the real
	// geometry. The bounding sphere is, contrary
	// to all other bounding volumes, in world space,
	// meaning that the ray does not need to be transformed
	// into object space for this first test
	if(!m_boundingSphere.rayCast(ray_))
		return result;

	matrix4x4 t = transform();
	matrix4x4 invT = math::inverse(t);
	ray objectRay = math::transform(invT, ray_);

	// Go through all objects and find the closest hit
	boost::optional<float_t> param;
	boost::optional<float_t> tmp;
	foreach(const sphere& sphere_, m_spheres)
	{
		if(tmp = sphere_.rayCastParameter(objectRay))
			if(!param || *tmp < *param)
				param = tmp;
	}

	foreach(const obb& box, m_boxes)
	{
		if(tmp = box.rayCastParameter(objectRay))
		if(!param || *tmp < *param)
			param = tmp;
	}

	if(param)
	{
		// The great thing about only using the parameter is that we don't need to do an additional
		// transform: Instead we simply use the world-ray to calculate our point (the transformation
		// is linear, which means that using the object-space-ray with the same parameter and then
		// transforming it to world-space MUST result in the same point as doing it in world-space
		// directly)
		result = ActorRayCast();
		result->parameter = *param;
		result->position = ray_.point(*param);

		// TODO: Calculate normal
		//result->normal   = ray_
	}

	return result;
}
///////////////////////////////////////////////////////////////////////////////////////////////////

void TPhysicsActor::debugRender(MeshVertices& vertices, Indices32 &indices, MeshVertices& tmpVertices, Indices32& tmpIndices) const
{
	// Unfortunately, all createXXX functions clear both buffers
	// so we have to explicitly merge the stuff
	// In addition to that: Our simulation uses double precision, rendering
	// single: so most matrices & vectors have to be transformed

	float4x4 thisT(transform());
	float3x3 thisA(m_state.rotation);

	foreach(const sphere& sphere_, m_spheres)
	{
		//createSphere(tmpVertices, tmpIndices, sphere.radius, 10);

		foreach(MeshVertex& vert, tmpVertices)
		{
			vert.position = math::transform(thisT, vert.position);
			vert.normal   = math::transform(thisA, vert.normal);
		}

		//merge(vertices, indices, tmpVertices, tmpIndices);
	}

	foreach(const obb& box, m_boxes)
	{
		//createCube(tmpVertices, tmpIndices);

		// The cube is a unit cube, but the box may not be a cube... transform it
		float4x4 s = float4x4::scale(math::float3(box.bounds));
		float4x4 t = s * float4x4(box.transform()) * thisT;
		float3x3 r = float3x3(box.rotation) * thisA;

		foreach(MeshVertex& vert, tmpVertices)
		{
			vert.position = math::transform(t, vert.position);
			vert.normal   = math::transform(r, vert.normal);
		}

		//merge(vertices, indices, tmpVertices, tmpIndices);
	}
}
///////////////////////////////////////////////////////////////////////////////////////////////////
